/** 
 * sampleAndDetect.c - The C source file of the task
 * NOTE: This file is based on C99 and rt-thread
 *      该文件实现的任务有：
 *      - 陀螺仪数据定采样
 *      - 超声距离检测
 *
 * Copyright (c) 2020-, garbage classify Project
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author            Notes           mail
 * 2021-04-02     江榕煜       first version   2373180028@qq.com
**/

#include "system.h"
#include "BSP.h"

#include "uart4Host.h"

#ifdef __USER_DEBUG__   // 全局调试打印使能
    #define __USER_DEBUG__SAMP__
#endif
#define __USER_DEBUG__SAMP__

ALIGN(RT_ALIGN_SIZE)
static struct rt_timer timer_sample;    // 定采样，触发定时器



WT931_9AxisData * IMU = NULL;
int32_t wheelSpeed_Left = 0,wheelSpeed_Right = 0;   // 滤波后的速度，供外部使用
int32_t wheelSpeed_LeftNow,wheelSpeed_RightNow;
float speed_FilterK = 0.2;

ALIGN(RT_ALIGN_SIZE)
static struct rt_thread thread_sample;  // 采样任务线程
static char stack_sample[1024];     // 采样线程栈空间

/** @brief  采样线程
 *  @note   
 *  @author 江榕煜（2021.4.2）
 *  @param 
 *  @retval
**/
static void thread_sample_Entry(void* entryParam)
{
    while(1)
    {
        wheelSpeed_LeftNow  = ABcoder_getSpeed(0);
        wheelSpeed_RightNow = -ABcoder_getSpeed(1);
        
        wheelSpeed_Left = speed_FilterK * wheelSpeed_LeftNow + (1-speed_FilterK)*wheelSpeed_Left;
        wheelSpeed_Right = speed_FilterK * wheelSpeed_RightNow + (1-speed_FilterK)*wheelSpeed_Right;

        uart4Host_feedback();
        
        rt_thread_suspend(rt_thread_self());    // 单次采样后挂起
        rt_schedule();
    }
}

/** @brief  采样定时器回调函数
 *  @note   
 *  @author 江榕煜（2021.4.2）
 *  @param 
 *  @retval
**/
static void timeout_sample_Entry(void* entryParam)
{
    rt_thread_resume(&thread_sample);   //恢复采样线程（触发单次采样）
}

/** @brief  数据采样线程初始化
 *  @note   
 *  @author 江榕煜（2021.4.9）
 *  @param  
 *          IMUptr(WT931_9AxisData *)   陀螺仪数据区指针
 *  @retval
 *          None
**/
void thread_sample_Init(WT931_9AxisData * IMUptr)
{
    IMU = IMUptr;
    
    /* 静态线程初始化 */
    rt_thread_init(&thread_sample,
                    "sample thread",
                    thread_sample_Entry,
                    RT_NULL,
                    &stack_sample[0],
                    sizeof(stack_sample),
                    0,10);
    
    /* 初始化定时器 */
    rt_timer_init(&timer_sample, "sample timer",  /* 定时器名字是 timer_S */
                    timeout_sample_Entry, /* 超时时回调的处理函数 */
                    RT_NULL, /* 超时函数的入口参数，传入采样函数线程句柄 */
                    20, /* 定时长度，以 OS Tick 为单位，即 20 个 OS Tick */
                    RT_TIMER_FLAG_PERIODIC); /* 周期性定时器 */
    
    rt_thread_startup(&thread_sample);  //启动线程
    rt_kprintf("sample-thread start success!\n");
                    
    rt_timer_start(&timer_sample);      //启动定时器
    rt_kprintf("sample-timer start success!\n");
}
#ifdef __USER_DEBUG__SAMP__
/** @brief  IMU数据打印
 *  @note   测试用
 *  @author 江榕煜（2021.3.19）
 *  @param  None
 *  @retval None
**/
int IMU_print()
{
    rt_kprintf("--------------------\n");
    rt_kprintf("the time is: %d:%d:%d\n",
                IMU->Time.ucHour,
                IMU->Time.ucMinute,
                IMU->Time.ucSecond);
    rt_kprintf("the accelerate is: %d,%d,%d\n",
                IMU->Accelerate.a[0],
                IMU->Accelerate.a[1],
                IMU->Accelerate.a[2]);
    rt_kprintf("the angle is: %d,%d,%d\n",
                IMU->Angle.Angle[0],
                IMU->Angle.Angle[1],
                IMU->Angle.Angle[2]);
    rt_kprintf("the gyro is: %d,%d,%d\n",
                IMU->Gyro.w[0],
                IMU->Gyro.w[1],
                IMU->Gyro.w[2]);
    rt_kprintf("--------------------\n");
  
    return 0;
}

MSH_CMD_EXPORT(IMU_print, print the data of IMU);

#endif
